#ifndef __MATRIX_H__
#define __MATRIX_H__
#include <iostream>
#include <algorithm>
#include <string>
#include <sstream>

#ifndef CASSERT
#define CASSERT( exp, name ) typedef int dummy##name [ (exp ) ? 1 : -1 ];
#endif

namespace algebra
{
	/** Exception used to signal errors in Matrix manipulation */
	class MatrixIndexOutOfBoundsException: public std::exception
	{
		public:
			/** Constructor. Specifies the matrix indices, and the ones that have triggered the exception */
			MatrixIndexOutOfBoundsException(int indexx, int indexy, int maxx, int maxy)
			{
				std::stringstream out;
				out << "Matrix index out of bounds, accessed item ["<< indexx<<","<<indexy<<"] when matrix is of size "<<maxx<<"x"<<maxy;
				mess = out.str();
			}
			/** Exception message */
			const char * what () const throw ()
			{
				return mess.c_str();
			}
			/** Destructor */
			~MatrixIndexOutOfBoundsException() throw(){}
		private:
			std::string mess;
	};

	/** Template used for matrices of any size. */
	template <typename T, int size_x, int size_y>
		class Matrix
		{
			public:
				/** Constructor. Creates the zero matrix */
				Matrix()
				{
					CASSERT( size_x > 0, matrix_width_is_more_than_zero);
					CASSERT( size_y > 0, matrix_height_is_more_than_zero);
					std::fill(m_data,m_data+(size_x*size_y), 0);
				}

				/** Copy constructor */
				Matrix(const Matrix<T,size_x,size_y> &other)
				{
					std::copy(other.m_data, other.m_data+(size_x*size_y), m_data);
				}
				
				/** Constructs the matrix from an array of values. 
				  @note It does no length checking, so the user must do it */
				Matrix(const T * array) // column major array
				{
					std::copy(array, array+(size_x*size_y), m_data);
				}

				/** Retrieves a column as a column vector*/
				Matrix<T,1,size_y> getCol(int numCol)
				{
					Matrix<T,1,size_y> result;
					for(int y=0;y<size_y;++y)
						result.at(0,y) = this->at(numCol,y);
					return result;
				}
				/** Retrieves a row as a row vector */
				Matrix<T,size_x,1> getRow(int numRow)
				{
					Matrix<T,size_x,1> result;
					for(int x=0;x<size_x;++x)
						result.at(x,0) = this->at(x,numRow);
					return result;
				}
				/** Gets the number of rows */
				inline int rows()const{return size_y;}
				/** Gets the number of columns */
				inline int cols()const{return size_x;}
				
				
				/** Gets the value for a matrix element (read-write). 
				  @note Coordinates are like if it was a C array: from (0,0) to (size_x-1,size_y-1) 
				  @throws MatrixIndexOutOfBoundsException Throws when the indices are out of the matrix size
				 */
				inline T& at(int col, int row)
				{
					if(row>=size_y || row<0 || col>=size_x || col<0)
						throw MatrixIndexOutOfBoundsException(col,row, size_x, size_y);

					return m_data[row+col*size_y];
				} 
				
				/** Gets the value for a matrix element (read-only). 
				  @note Coordinates are like if it was a C array: from (0,0) to (size_x-1,size_y-1) 
				  @throws MatrixIndexOutOfBoundsException Throws when the indices are out of the matrix size
				 */
				inline const T& at(int col, int row)const
				{
					if(row>=size_y || row<0 || col>=size_x || col<0)
						throw MatrixIndexOutOfBoundsException(col,row, size_x, size_y);

					return m_data[row+col*size_y];
				}
				/** Accesses a matrix element value as if this was a one-dimensional array (read-write)*/
				inline T& operator[](int index)
				{
					return m_data[index];
				}
				/** Accesses a matrix element value as if this was a one-dimensional array (read-only)*/
				inline const T& operator[](int index) const
				{
					return m_data[index];
				}	

				/** Multiplies this matrix with another and returns the result*/
				template<int other_x>
					Matrix<T, other_x,size_y> operator*(const Matrix<T,other_x,size_x>& other) const // it must havee the same rows as this has cols
					{
						Matrix<T,other_x,size_y> result;

						for(int fx = 0; fx<other_x; ++fx)
							for(int fy = 0;fy<size_y; ++fy)
							{
								T sumation = 0;
								for(int thisx = 0;thisx<size_x;++thisx)
									sumation += this->at(thisx,fy) * other.at(fx,thisx);
								result.at(fx,fy) = sumation;
							}
						return result;
					}

				/** Multiplies this matrix by a scalar and returns the result*/
				Matrix<T,size_x,size_y> operator*(T scalar) const
				{
					Matrix<T,size_x,size_y> result(*this);
					result *= scalar;
					return result;
				}

				/** Multiplies this matrix by a scalar */
				Matrix<T,size_x,size_y>& operator*=(T scalar)
				{
					for(int y=0;y<size_y;++y)
						for(int x=0;x<size_x;++x)
							this->at(x,y) *=scalar;
					return *this;
				}

				/** Divides this matrix by a scalar and returns the result*/
				Matrix<T,size_x,size_y> operator/(T scalar) const
				{
					Matrix<T,size_x,size_y> result(*this);
					result /= scalar;
					return result;
				}

				/** Divides this matrix by a scalar */
				Matrix<T,size_x,size_y>& operator/=(T scalar)
				{
					for(int y=0;y<size_y;++y)
						for(int x=0;x<size_x;++x)
							this->at(x,y) /=scalar;
					return *this;
				}

				/** Adds another matrix and this one, and returns the result */
				Matrix<T,size_x,size_y> operator+(const Matrix<T,size_x,size_y>& other) const
				{
					Matrix<T,size_x,size_y> result(*this);
					result+= other;
					return result;
				}

				/** Adds another matrix to this one */
				Matrix<T,size_x,size_y>& operator+=(const Matrix<T,size_x,size_y>& other)
				{
					for(int y=0;y<size_y;++y)
						for(int x=0;x<size_x;++x)
							this->at(x,y) += other.at(x,y);
					return *this;
				}

				/** Substracts another matrix from this one, and returns the result */
				Matrix<T,size_x,size_y> operator-(const Matrix<T,size_x,size_y>& other) const
				{
					Matrix<T,size_x,size_y> result(*this);
					result-= other;
					return result;
				}
				/** Substracts another matrix from this one */
				Matrix<T,size_x,size_y>& operator-=(const Matrix<T,size_x,size_y>& other)
				{
					for(int y=0;y<size_y;++y)
						for(int x=0;x<size_x;++x)
							this->at(x,y) -= other.at(x,y);
					return *this;
				}
				/** Gets the transposed matrix */
				Matrix<T,size_y,size_x> transposed() const
				{
					Matrix<T,size_y,size_x> result;
					for(int y=0;y<size_y;++y)
						for(int x=0;x<size_x;++x)
							result.at(y,x) = this->at(x,y);
					return result;
				}

				/** Checks if two matrices are different */
				bool operator!=(const Matrix<T,size_x,size_y> & other) const
				{
					return !(*this == other);
				}
				/** Checks if two matrices are equal */
				bool operator==(const Matrix<T,size_x,size_y> & other) const
				{
					for(int y=0;y<size_y;++y)
						for(int x=0;x<size_x;++x)
							if(other.at(x,y) != this->at(x,y))
								return false;

					return true;
				}
				/** Sets the values in a column, getting them from a column vector */
				void setColumn(int columnIndex, const Matrix<T,1,size_y> & column)
				{
					for(int i=0;i<size_y;++i)
						this->at(columnIndex, i) = column.at(0,i);
				}
				/** Sets the values in a row, getting them from a row vector */
				void setRow( int rowIndex, const Matrix<T,size_x,1> & row)
				{
					for(int i=0;i<size_x;++i)
						this->at(i,rowIndex) = row.at(i,0);
				}

			private:
				T m_data[size_x*size_y];

		};
	/* Returns the identity matrix for the given dimensions and type */
	template<typename T, int size> 
		Matrix<T,size,size> identityMatrix()
		{
			Matrix<T,size,size> result;
			for(int i=0;i<size;++i)
				result.at(i,i) = 1;
			return result;
		}
	/** Writes a matrix to an output stream */
	template<typename T, int size_y, int size_x>
		std::ostream & operator<<(std::ostream &out, const Matrix<T,size_y,size_x> &m)
		{
			out << std::endl<<"Matrix:("<<size_y<<"x"<<size_x<<")"<<std::endl;
			for(int y=0;y<size_x;++y)
			{
				out<<"|";
				out<<m.at(0,y);
				for(int x=1;x<size_y;++x)
				{
					out<<", "<<m.at(x,y);
				}
				out<<"|"<<std::endl;
			}

			return out;
		}


	/** Multiplies a matrix by a scalar */
	template<typename T, int size_x, int size_y>
		Matrix<T,size_x,size_y> operator*(T scalar, const Matrix<T,size_x,size_y> &m)
		{	
			return m*scalar;
		}

	/** Type of 4x4 float matrices */
	typedef Matrix<float,4,4> Matrix4f;
	/** Type of 3x3 float matrices */
	typedef Matrix<float,3,3> Matrix3f;
	/** Type of 2x2 float matrices */
	typedef Matrix<float,2,2> Matrix2f;
	
	/** Type of 4 elements float column vector*/
	typedef Matrix<float,1,4> Vector4f;
	/** Type of 3 elements float column vector*/
	typedef Matrix<float,1,3> Vector3f;
	/** Type of 2 elements float column vector*/
	typedef Matrix<float,1,2> Vector2f;

	/** Type of 4 elements float column vector*/
	typedef Matrix<float,1,4> Column4f;
	/** Type of 3 elements float column vector*/
	typedef Matrix<float,1,3> Column3f;
	/** Type of 2 elements float column vector*/
	typedef Matrix<float,1,2> Column2f;

	/** Type of 4 elements float row vector*/
	typedef Matrix<float,4,1> Row4f;
	/** Type of 3 elements float row vector*/
	typedef Matrix<float,3,1> Row3f;
	/** Type of 2 elements float row vector*/
	typedef Matrix<float,2,1> Row2f;

	/** Gets the 4x4 float identity matrix */
	Matrix4f identityMatrix4f();
}

#endif

